#include <Arduino.h>
#include "control/motor_controller.h"
#include "control/differential_drive.h"
#include "control/omni_drive.h"
#include "control/steering_drive.h"
#include "sensor/sensor_manager.h"
#include "config_manager.h"
#include "vehicle_system.h"

// 引脚定义
// 电机引脚
#define L1 5
#define L2 6
#define R1 9
#define R2 10
// 万向轮额外引脚
#define FL1 2
#define FL2 3
#define FR1 4
#define FR2 5
#define BL1 6
#define BL2 7
#define BR1 8
#define BR2 9
// 舵机引脚
#define STEERING_PIN 11
// 传感器引脚
#define TRIG_PIN 12
#define ECHO_PIN 13
#define TEMP_PIN A0

// 创建控制器实例
ConfigManager configManager;
SensorManager sensorManager(TRIG_PIN, ECHO_PIN, TEMP_PIN);

// 根据配置创建电机控制器
MotorController* createMotorController() {
  String driveMode = configManager.getDriveMode();
  if (driveMode == "OmniDrive") {
    return new OmniDrive(FL1, FL2, FR1, FR2, BL1, BL2, BR1, BR2);
  } else if (driveMode == "SteeringDrive") {
    return new SteeringDrive(L1, L2, R1, R2, STEERING_PIN);
  } else {
    // 默认差速驱动
    return new DifferentialDrive(L1, L2, R1, R2);
  }
}

MotorController* motorController = createMotorController();
VehicleSystem vehicleSystem(motorController, &sensorManager, &configManager);

void setup() {
  vehicleSystem.init();
}

void loop() {
  vehicleSystem.loop();
}